/*
 *       AP_MotorsQuad.cpp - ArduCopter motors library
 *       Code by RandyMackay. DIYDrones.com
 *
 *       This library is free software; you can redistribute it and/or
 *   modify it under the terms of the GNU Lesser General Public
 *   License as published by the Free Software Foundation; either
 *   version 2.1 of the License, or (at your option) any later version.
 */

#include "AP_MotorsQuad_dc.h"

// setup_motors - configures the motors for a quad
void AP_MotorsQuad_dc::setup_motors()
{
    // call parent
    AP_MotorsMatrix_dc::setup_motors();

    // hard coded config for supported frames
    if( _frame_orientation == AP_MOTORS_PLUS_FRAME ) {
        // plus frame set-up
        add_motor(AP_MOTORS_MOT_1,  90, AP_MOTORS_MATRIX_MOTOR_CCW, 2);
        add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_MOTOR_CCW, 4);
        add_motor(AP_MOTORS_MOT_3,   0, AP_MOTORS_MATRIX_MOTOR_CW,  1);
        add_motor(AP_MOTORS_MOT_4, 180, AP_MOTORS_MATRIX_MOTOR_CW,  3);
    }
	else //if( _frame_orientation == AP_MOTORS_NEW_FRAME)
	{
		add_motor(AP_MOTORS_MOT_1,   45, AP_MOTORS_MATRIX_MOTOR_CCW, 1,	AP_MotorsMatrix_dc::besc);		// Big Propeller 1
        add_motor(AP_MOTORS_MOT_2, -135, AP_MOTORS_MATRIX_MOTOR_CCW, 3,	AP_MotorsMatrix_dc::besc);		// Big Propeller 2
        add_motor(AP_MOTORS_MOT_3,  -45, AP_MOTORS_MATRIX_MOTOR_CW,  4,  AP_MotorsMatrix_dc::besc);		// Big Propeller 3
        add_motor(AP_MOTORS_MOT_4,  135, AP_MOTORS_MATRIX_MOTOR_CW,  2,  AP_MotorsMatrix_dc::besc);		// Big Propeller 4
		add_motor(AP_MOTORS_MOT_5,    0, AP_MOTORS_MATRIX_MOTOR_CCW, 5,	AP_MotorsMatrix_dc::lesc);		// Little Propeller 5
        add_motor(AP_MOTORS_MOT_6,    0, AP_MOTORS_MATRIX_MOTOR_CCW, 7,	AP_MotorsMatrix_dc::lesc);		// Little Propeller 6
        add_motor(AP_MOTORS_MOT_7,    0, AP_MOTORS_MATRIX_MOTOR_CW,  8,	AP_MotorsMatrix_dc::servo_roll);		// SERVO 1	Roll
        add_motor(AP_MOTORS_MOT_8,    0, AP_MOTORS_MATRIX_MOTOR_CW,  6,	AP_MotorsMatrix_dc::servo_roll);		// SERVO 2	Roll
		add_motor(AP_MOTORS_MOT_9,    0, AP_MOTORS_MATRIX_MOTOR_CCW, 9,	AP_MotorsMatrix_dc::servo_pitch);		// SERVO 3	Pitch + Yaw
        add_motor(AP_MOTORS_MOT_10,   0, AP_MOTORS_MATRIX_MOTOR_CCW,10,  AP_MotorsMatrix_dc::servo_yaw);		// SERVO 4	Pitch + Yaw
	}
}
